//==============================================================
// Copyright (C) 2004 Danny Chapman 
//               danny@rowlhouse.freeserve.co.uk
//--------------------------------------------------------------
//               
/// @file intersection.inl 
//                     
//==============================================================

namespace Physics
{
#if 0
	//====================================================================
	// SegmenPlaneIntersection
	//====================================================================
	inline bool SegmenPlaneIntersection(float* tS, const Segment& seg, const Plane& plane)
	{
		float denom = Dot(plane.GetN(), seg.GetDelta());
		if (Abs(denom) > SCALAR_TINY)
		{
			float t = -(Dot(plane.GetN(), seg.GetOrigin()) + plane.GetD()) / denom;
			if (t < 0.0f || t > 1.0f)
				return false;
			if (tS) *tS = t;
			return true;
		}
		else
		{
			// parallel - return false even if it's in the plane
			return false;
		}
	}

	//====================================================================
	// SegmenTriangleIntersection
	// See Real-Time Rendering p581
	//====================================================================
	inline bool SegmenTriangleIntersection(float* tS, float* tT0, float* tT1, 
		const Segment& seg, const Triangle& triangle)
	{
		/// the parameters - if hit then they get copied into the args
		float u, v, t;

		const Vector3& e1 = triangle.GetEdge0();
		const Vector3& e2 = triangle.GetEdge1();
		Vector3 p = Cross(seg.GetDelta(), e2);
		float a = Dot(e1, p);
		if (a > -SCALAR_TINY && a < SCALAR_TINY)
			return false;
		float f = 1.0f / a;
		Vector3 s = seg.GetOrigin() - triangle.GetOrigin();
		u = f * Dot(s, p);
		if (u < 0.0f || u > 1.0f)
			return false;
		Vector3 q = Cross(s, e1);
		v = f * Dot(seg.GetDelta(), q);
		if (v < 0.0f || (u + v) > 1.0f)
			return false;
		t = f * Dot(e2, q);
		if (t < 0.0f || t > 1.0f)
			return false;
		if (tS) *tS = t;
		if (tT0) *tT0 = u;
		if (tT1) *tT1 = v;
		return true;
	}
#endif
	//==============================================================
	// SegmentSphereIntersection
	//==============================================================
	inline bool SegmentSphereIntersection(float* tS, const Segment& seg, const Sphere& sphere)
	{
		const Vector3& r = seg.Delta;
		const Vector3 s = seg.Origin - sphere.getPos();
		float radiusSq = Square(sphere.getRadius());
		float rSq = r.SquareLength();
		if (rSq < radiusSq)
		{
			if (tS) *tS = 0.0f; // starting inside
			return true;
		}

		float sDotr = s | r;
		float sSq = s.SquareLength();
		float sigma = Square(sDotr) - rSq * (sSq - radiusSq);
		if (sigma < 0.0f)
			return false;
		float sigmaSqrt = sqrt(sigma);
		float lambda1 = (-sDotr - sigmaSqrt) / rSq;
		float lambda2 = (-sDotr + sigmaSqrt) / rSq;
		if (lambda1 > 1.0f || lambda2 < 0.0f)
			return false;
		// intersection!
		if (tS) *tS = Max(lambda1, 0.0f);
		return true;
	}

	//==============================================================
	// SegmentInfiniteCylinderIntersection
	//==============================================================
	inline bool SegmentInfiniteCylinderIntersection(float* tS, const Segment& seg, const Segment& cylinderAxis, float radius)
	{
		const Vector3& Ks = seg.Delta;
		const float kss = (Ks | Ks);
		const float radiusSq = Square(radius);

		const Vector3& Ke = cylinderAxis.Delta;
		const Vector3  Kg = cylinderAxis.Origin - seg.Origin;

		const float kee = (Ke | Ke);
		if (Abs(kee) < SCALAR_TINY)
			return false;

		const float kes = (Ke | Ks);
		const float kgs = (Kg | Ks);
		const float keg = (Ke | Kg);
		const float kgg = (Kg | Kg);

		// check if start is inside
		float distSq = (Kg - (Ke * keg) / kee).SquareLength();
		if (distSq < radiusSq)
		{
			if (tS) *tS = 0.0f;
			return true;
		}

		// a * t^2 + b * t + c = 0
		const float a = kee * kss - Square(kes);
		if (Abs(a) < SCALAR_TINY)
			return false;
		const float b = 2.0f * (keg * kes - kee * kgs);
		const float c = kee * (kgg - radiusSq) - Square(keg);

		float blah = Square(b) - 4.0f * a * c;
		if (blah < 0.0f)
			return false;

		// solve for t - take minimum
		const float t = (-b - sqrt(blah)) / (2.0f * a);

		if (t < 0.0f || t > 1.0f)
			return false;

		if (tS) *tS = t;

		return true;
	}


#if 1
	//==============================================================
	// SegmentCapsuleIntersection
	//==============================================================
	inline bool SegmentCapsuleIntersection(float* tS, const Segment& seg, const Capsule& capsule)
	{
		float bestFrac = SCALAR_HUGE;

		// do the main sides
		float sideFrac = SCALAR_HUGE;
		if (!SegmentInfiniteCylinderIntersection(&sideFrac, seg, 
			Segment(capsule.getTransform().getPos(), capsule.getTransform().getLook()), 
			capsule.getRadius()))
			return false;

		// only keep this if the side intersection point is within the capsule segment ends
		Vector3 sidePos = seg.GetPoint(sideFrac);
		if (((sidePos - capsule.getTransform().getPos()) | capsule.getTransform().getLook()) < 0.0f)
			sideFrac = SCALAR_HUGE;
		else if (((sidePos - capsule.GetEnd()) | capsule.getTransform().getLook()) > 0.0f)
			sideFrac = SCALAR_HUGE;

		// do the two ends
		float originFrac = SCALAR_HUGE;
		SegmentSphereIntersection(&originFrac, seg, Sphere(capsule.getTransform().getPos(), capsule.getRadius()));
		float endFrac = SCALAR_HUGE;
		SegmentSphereIntersection(&originFrac, seg, Sphere(capsule.GetEnd(), capsule.getRadius()));

		bestFrac = Min3(sideFrac, originFrac, endFrac);
		if (bestFrac <= 1.0f)
		{
			if (tS) *tS = bestFrac;
			return true;
		}
		return false;
	}
#endif

#if 0

	//====================================================================
	// SwepSpherePlaneIntersection
	//====================================================================
	inline bool SwepSpherePlaneIntersection(Vector3& pt, float& finalPenetration,
		const Sphere& oldSphere, const Sphere& newSphere, 
		const Plane& plane, 
		float* pOldDistToPlane, float* pNewDistToPlane)
	{
		const float oldDistToPlane = pOldDistToPlane ? *pOldDistToPlane : PoinPlaneDistance(oldSphere.GetPos(), plane);
		const float newDistToPlane = pNewDistToPlane ? *pNewDistToPlane : PoinPlaneDistance(newSphere.GetPos(), plane);
		const float radius = oldSphere.GetRadius();

		if (newDistToPlane >= oldDistToPlane)
			return false;
		if (newDistToPlane > radius)
			return false;

		// intersect with plane
		float t = (newDistToPlane - radius) / (newDistToPlane - oldDistToPlane);
		if (t < 0.0f || t > 1.0f)
			return false;

		pt = oldSphere.GetPos() + t * (newSphere.GetPos() - oldSphere.GetPos()) - Min(radius, oldDistToPlane) * plane.GetN();
		finalPenetration = radius - newDistToPlane;
		return true;
	}
#endif

};